#include "foc.h"

void inverse_clarke(int32_t alpha, int32_t beta, int32_t *a, int32_t *b, int32_t *c)
{
	*a = alpha;
	*b = (-8192 * alpha + 14189 * beta) >> 14; // 0.8660254 = sqrt(3)/2
	*c = (-8192 * alpha - 14189 * beta) >> 14;
}

void inverse_park(int32_t d, int32_t q, float theta, int32_t *alpha, int32_t *beta)
{
	int32_t sin_val = sinf(theta) * 32768;
	int32_t cos_val = cosf(theta) * 32768;

	*alpha = d * cos_val - q * sin_val;
	*beta = d * sin_val + q * cos_val;
	*alpha >>= 15;
	*beta >>= 15;
}

float machinery_angle_to_electric_angle(foc_t *foc, float angle)
{
	foc->angle = angle * foc->k + foc->b;
	return foc->angle;
}

void foc_init(foc_t *foc, int *angle, int mid_angle)
{
	// 校准
	float temp_angle_x[48] = {0};
	float temp_angle_y[48] = {0, 2.0 / 3.0 * FOC_PI, 4.0 / 3.0 * FOC_PI};
	foc->set_va_pwm((0 % 3 == 0) ? (foc->initv) : (0));
	foc->set_vb_pwm((0 % 3 == 1) ? (foc->initv) : (0));
	foc->set_vc_pwm((0 % 3 == 2) ? (foc->initv) : (0));
	foc->delay(foc->delay_x);
	float last_angle = *angle;
	float now_angle = *angle;
	uint8_t idx = 0;
	int8_t flag = 0;
	while (1)
	{
		foc->set_va_pwm((idx % 3 == 0) ? (foc->initv) : (0));
		foc->set_vb_pwm((idx % 3 == 1) ? (foc->initv) : (0));
		foc->set_vc_pwm((idx % 3 == 2) ? (foc->initv) : (0));
		foc->delay(foc->delay_x);
		last_angle = now_angle;
		now_angle = *angle;
		float tempe = now_angle - last_angle;
		if (tempe > mid_angle)
		{
			flag = -1;
		}
		else if (tempe < -mid_angle)
		{
			flag = 1;
		}
		temp_angle_x[idx] = *angle + ((mid_angle << 1) * flag);
		temp_angle_y[idx] = idx * 2.094395;
		if (idx >= 48)
			break;
		if (abs(temp_angle_x[idx] - temp_angle_x[0]) >= mid_angle)
			break;
		idx++;
	}

	float sum_xy = 0, sum_x = 0, sum_y = 0, sum_pow_x = 0, pow_sum_x = 0;
	for (int i = 0; i < idx; i++)
	{
		sum_xy += temp_angle_x[i] * temp_angle_y[i];
		sum_x += temp_angle_x[i];
		sum_y += temp_angle_y[i];
		pow_sum_x += temp_angle_x[i] * temp_angle_x[i];
	}
	sum_pow_x = sum_x * sum_x;
	foc->k = (idx * sum_xy - sum_x * sum_y) / (idx * pow_sum_x - sum_pow_x);
	foc->b = (sum_y - foc->k * sum_x) / idx;
}

int32_t zero_processing(int32_t target, int32_t fb, int32_t midfb)
{
	int32_t e = target - fb;
	if (e > midfb)
	{
		return fb + (midfb << 1);
	}
	else if (e < -midfb)
	{
		return fb - (midfb << 1);
	}
	return fb;
}
